#pragma once

#include "PastyCam.h"
//#include <math.h>
#include <cmath>
#include <Vec3f.hpp>
using siut::simd::Vec3f;

#define BEHIND		0
#define INTERSECTS	1
#define FRONT		2

#ifndef isnan
inline bool isnan(double x) {
	return x != x;
}
#endif

class CollideableCamera : public PastyCam
{
private:
	float m_radius;
public:





	float PlaneDistance(Vec3f Normal, Vec3f Point)
	{	
		float distance = 0;									// This variable holds the distance from the plane tot he origin

		// Use the plane equation to find the distance (Ax + By + Cz + D = 0)  We want to find D.
		// So, we come up with D = -(Ax + By + Cz)
		// Basically, the negated dot product of the normal of the plane and the point. (More about the dot product in another tutorial)
		distance = - dot( Normal, Point );

		return distance;									// Return the distance
	}

	int ClassifySphere(Vec3f &vCenter, 
		Vec3f &vNormal, Vec3f &vPoint, float radius, float &distance)
	{
		// First we need to find the distance our polygon plane is from the origin.
		float d = (float)PlaneDistance(vNormal, vPoint);

		// Here we use the famous distance formula to find the distance the center point
		// of the sphere is from the polygon's plane.  
		distance = (vNormal.x() * vCenter.x() + vNormal.y() * vCenter.y() + vNormal.z() * vCenter.z() + d);

		// If the absolute value of the distance we just found is less than the radius, 
		// the sphere intersected the plane.
		if( fabs(distance) < radius)
			return INTERSECTS;
		// Else, if the distance is greater than or equal to the radius, the sphere is
		// completely in FRONT of the plane.
		else if(distance >= radius)
			return FRONT;

		// If the sphere isn't intersecting or in FRONT of the plane, it must be BEHIND
		return BEHIND;
	}

	double AngleBetweenVectors(Vec3f Vector1, Vec3f Vector2)
	{							
		// Get the dot product of the vectors
		float dotProduct = dot( Vector1, Vector2 );				

		// Get the product of both of the vectors magnitudes
		float vectorsMagnitude = length(Vector1) * length(Vector2);
		// Get the angle in radians between the 2 vectors
		double angle = acos( dotProduct / vectorsMagnitude );

		// Here we make sure that the angle is not a -1.#IND0000000 number, which means indefinate
		if(isnan(angle))
			return 0;

		// Return the angle in radians
		return( angle );
	}

	// Faster tests do exits...
	bool InsidePolygon(Vec3f vIntersection, Vec3f Poly[], long verticeCount)
	{
		const double MATCH_FACTOR = 0.99;		// Used to cover up the error in floating point
		double Angle = 0.0;						// Initialize the angle
		Vec3f vA, vB;						// Create temp vectors

		for (int i = 0; i < verticeCount; i++)		// Go in a circle to each vertex and get the angle between
		{	
			vA = Poly[i] - vIntersection;			// Subtract the intersection point from the current vertex
			// Subtract the point from the next vertex
			vB = Poly[(i + 1) % verticeCount] - vIntersection;

			Angle += AngleBetweenVectors(vA, vB);	// Find the angle between the 2 vectors and add them all up as we go along
		}

		if(Angle >= (MATCH_FACTOR * (2.0 * M_PI)) )	// If the angle is greater than 2 PI, (360 degrees)
			return true;							// The point is inside of the polygon

		return false;								// If you get here, it obviously wasn't inside the polygon, so Return FALSE
	}

	Vec3f ClosestPointOnLine(Vec3f vA, Vec3f vB, Vec3f vPoint)
	{
		// Create the vector from end point vA to our point vPoint.
		Vec3f vVector1 = vPoint - vA;

		// Create a normalized direction vector from end point vA to end point vB
		Vec3f vVector2 = vB - vA;
		vVector2.normalize();

		// Use the distance formula to find the distance of the line segment (or magnitude)
		float d = length(vA - vB);

		// Using the dot product, we project the vVector1 onto the vector vVector2.
		// This essentially gives us the distance from our projected vector from vA.
		float t = vVector2.dot(vVector1);

		// If our projected distance from vA, "t", is less than or equal to 0, it must
		// be closest to the end point vA.  We want to return this end point.
		if (t <= 0) 
			return vA;

		// If our projected distance from vA, "t", is greater than or equal to the magnitude
		// or distance of the line segment, it must be closest to the end point vB.  So, return vB.
		if (t >= d) 
			return vB;

		// Here we create a vector that is of length t and in the direction of vVector2
		Vec3f vVector3 = vVector2 * t;

		// To find the closest point on the line segment, we just add vVector3 to the original
		// end point vA.  
		Vec3f vClosestPoint = vA + vVector3;

		// Return the closest point on the line segment
		return vClosestPoint;
	}

	bool EdgeSphereCollision(Vec3f &vCenter, 
		Vec3f vPolygon[], int vertexCount, float radius)
	{
		Vec3f vPoint;

		// This function takes in the sphere's center, the polygon's vertices, the vertex count
		// and the radius of the sphere.  We will return true from this function if the sphere
		// is intersecting any of the edges of the polygon.  

		// Go through all of the vertices's in the polygon
		for(int i = 0; i < vertexCount; i++)
		{
			// This returns the closest point on the current edge to the center of the sphere.
			vPoint = ClosestPointOnLine(vPolygon[i], vPolygon[(i + 1) % vertexCount], vCenter);

			// Now, we want to calculate the distance between the closest point and the center
			float distance = length(vPoint - vCenter);

			// If the distance is less than the radius, there must be a collision so return true
			if(distance < radius)
				return true;
		}

		// The was no intersection of the sphere and the edges of the polygon
		return false;
	}


	void checkCollision( Vec3f *pVertices, Vec3f *pNormals, int numVertices )
	{
		m_radius = 1.0f;

		for(int i = 0; i < numVertices; i += 3)
		{
			Vec3f vTriangle[3] = { pVertices[i], pVertices[i+1], pVertices[i+2] };
			Vec3f &vNormal = pNormals[i];

			float distanceFromPlane = 0.0f;
			Vec3f m_vPosition(posx, posy, posz);
			int classification = ClassifySphere(m_vPosition, vNormal, vTriangle[0], m_radius, distanceFromPlane);

			if(classification == INTERSECTS) 
			{
				// Now we want to project the sphere's center onto the triangle's plane
				Vec3f vOffset = vNormal * distanceFromPlane;
				Vec3f vIntersection = m_vPosition - vOffset;

				// Check if the intersection point is inside the triangles perimeter

				if(InsidePolygon(vIntersection, vTriangle, 3) ||
					EdgeSphereCollision(m_vPosition, vTriangle, 3, m_radius/2.0f))
				{

					Vec3f vOffset;
					if(distanceFromPlane > 0)
					{
						// Find the distance that our sphere is overlapping the plane, then
						// find the direction vector to move our sphere.
						float distanceOver = m_radius - distanceFromPlane;
						vOffset = vNormal * distanceOver;
					}
					else // Else colliding from behind the polygon
					{
						// Find the distance that our sphere is overlapping the plane, then
						// find the direction vector to move our sphere.
						float distanceOver = m_radius + distanceFromPlane;
						vOffset = vNormal * -distanceOver;
					}

					//vOffset = GetCollisionOffset(vNormal, m_radius, distance);

					// Now that we have the offset, we want to ADD it to the position and
					// view vector in our camera.  This pushes us back off of the plane.  We
					// don't see this happening because we check collision before we render
					// the scene.
					m_vPosition = m_vPosition + vOffset;
					posx = m_vPosition.x();
					posy = m_vPosition.y();
					posz = m_vPosition.z();
					update(0,0,0,0,0);
					//m_vView = m_vView + vOffset;
				}
		}
	}

	}
};